#include "WPILib.h"

/*
 * This is a simple program that is set up to drive using two joysticks and turn a motor
 * on and off using switches on the test board. The program also operates a pneumatics
 * test board using buttons on the xbox controller.
 * The autonomous mode is the default mode.
 * This program will be used to test our test board configuration and the new roboRIO.
 */

class Robot: public IterativeRobot
{

	// motor controllers
	Victor driveFLeft;
	Victor driveBLeft;
	Victor driveFRight;
	Talon driveBRight;
	Victor extraMotor;
	CANTalon encoderMotor;

	RobotDrive myRobot; // robot drive system

	Joystick rightStick; // right stick
	Joystick leftStick; // left stick
	Joystick xbox; // xbox controller
	Joystick launch; //launchpad board

	// digital inputs
	DigitalInput switch1;
	DigitalInput switch2;
	DigitalInput switch3;
	DigitalInput switch4;

	AnalogInput u;

	LiveWindow *lw;
	int autoLoopCounter;
	//Timer t;

	// spike relays
	Relay spike1;
	Relay spike2;

	// pneumatics
	Solenoid sol;
	Solenoid solRetract;
	Compressor comp;

	//AnalogInput ai;
	Gyro g;
	Encoder *e;

	bool on;
	bool pressed;

public:
	Robot() :
		driveFLeft(0), // PWM port 0
		driveBLeft(1), // PWM port 1
		driveFRight(2),// PWM port 2
		driveBRight(3),// PWM port 3
		extraMotor(4), // PWM port 4
		encoderMotor(2),// CAN Node 2
		myRobot(driveFLeft, driveBLeft, driveFRight, driveBRight),	// these must be initialized in the same order
		rightStick(0), // right joystick
		leftStick(1),  // left joystick
		xbox(2), // xbox controller
		launch(3), //launchpad
		switch1(0), // DIO port 0
		switch2(1), // DIO port 1
		switch3(2), // DIO port 2
		switch4(3), // DIO port 3
		u(1),
		lw(NULL),
		autoLoopCounter(0),
		spike1(0), // relay port 0
		spike2(1), // relay port 1
		sol(1,0), // CAN Node 1, solenoid launch port 0
		solRetract(1,1), // CAN Node 1, solenoid  retract port 1
		comp(1), // compressor CAN Node 1
		//ai(1),//Analog in port 1
		g(0)//Gyro in port 1

	{
		myRobot.SetExpiration(0.1);
	}

private:
	void RobotInit()
	{
		lw = LiveWindow::GetInstance();
		comp.Start();; // turn compressor on when robot initializes
		e = new Encoder(4, 5, false, Encoder::EncodingType::k4X);

		//usb camera code
		//CameraServer::GetInstance()->SetQuality(50);
		//the camera name (ex "cam0") can be found through the roborio web interface
		//CameraServer::GetInstance()->StartAutomaticCapture("cam0");
	}

	void AutonomousInit()
	{
		autoLoopCounter = 0;
		//t = new Timer();
		//t.start();
		//myRobot.TankDrive(.5, .5);
	}

	void AutonomousPeriodic()
	{
		//if (t.HasPeriodPassed(2.0)) { myRobot.TankDrive(0, 0); t.stop(); }

		//myRobot.TankDrive(0.5, 0.5) would probably be better; keeping motor polarity consistent would help, and is critical if its 2 motors going to a single gearbox
		if(autoLoopCounter < 100) //Check if we've completed 100 loops (approximately 2 seconds) time should probably be used instead of counting loops
		{
			//constantly telling the motors to 'set' to a new speed is best avoided if possible. not sure if it makes a difference or not, but it definitely does
			//when they are different values, and in different logical parts of code that seem unrelated, and you can't figure out why the robot is jittering
			myRobot.Drive(-0.5, 0.0); 	// drive forwards half speed
			autoLoopCounter++;
		}
		else myRobot.Drive(0.0, 0.0); 	// stop robot
	}

	void TeleopInit()
	{
		g.InitGyro(); //calibrates gyro
		on = false;
		pressed = false;
		encoderMotor.Set(.5);
	}

	void DisabledInit()
	{

	}

	void TeleopPeriodic()
	{



		myRobot.TankDrive(rightStick, leftStick); // drive with tank style (use two controllers)

		//encoderMotor.Set(switch1.Get() ? 1.0 : -1.0); // if switch enables turn motor on

		/*driveFLeft.SetSpeed(switch2.Get() ? 1.0 : 0.0); // if switch enables turn motor on
		driveBLeft.SetSpeed(switch3.Get() ? 1.0 : 0.0); // if switch enables turn motor on
		driveFRight.SetSpeed(switch4.Get() ? 1.0 : 0.0);// if switch enables turn motor on
		*/


		/*

		//controls pneumatics with xbox controller

		if(xbox.GetRawButton(1)) //		 launches solenoid piston when A button is pressed
		{
			sol.Set(true);
			solRetract.Set(false);
			//solRetract.Set(false);
		}

		if(xbox.GetRawButton(2)) // retracts solenoid piston when B button is pressed
		{
			sol.Set(false);
			solRetract.Set(true);
		}

		*/


		/*
		if(launch.GetRawButton(13)) {

			if(!pressed) {
				on = !on;
				encoderMotor.Set(on ? 1.0 : 0.0);
				spike2.Set(on ? Relay::Value::kForward : Relay::Value::kOff);
				SmartDashboard::PutString("DB/String 3",on ? "true" : "false");

			}
			pressed = true;
			SmartDashboard::PutString("DB/String 2", "Pressed");


		}
		else if(!launch.GetRawButton(13)) {
			pressed = false;
			SmartDashboard::PutString("DB/String 2", "Not Pressed");

		}
		 */


		if(xbox.GetRawButton(3)) {
			g.Reset();
		}

		SmartDashboard::PutString("DB/String 0", std::to_string(g.GetAngle()));//Displays angle on dashboard


		if(e->Get() >= 10000) {
			e->Reset();
			//encoderMotor.Set(0.0);
		}

		extraMotor.SetSpeed((g.GetAngle())/360);

		SmartDashboard::PutString("DB/String 1", std::to_string(u.GetVoltage()));
	}

	void TestPeriodic()
	{
		lw->Run();
	}
};

START_ROBOT_CLASS(Robot);
